#include "mqtt_comm.h"
#include "term.h"
#include <cJSON.h>
#include <mosquitto.h>
#include <pthread.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>

/* 定义全局变量：接收数据与发送数据 */
RobotControlData g_robotControlData = {{0}, {0}, 0, 0, 0};
RobotStateData g_robotStateData = {{0}, {0}, 0, -1, 0}; //初始化不能操作

/* 全局变量：保存当前用于正运动学的obj_pos */
double g_current_obj_pos[6] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};

/* 新增：重复指令标志（检测到重复 msg_id 时不发送 ACK，而是持续上报 status=103）
 */
static int duplicate_msg_active = 0; // 0=无重复, 1=正在上报103
static int duplicate_control_type =
    0; // 记录重复指令对应的控制类型（1=Device,2=Axis）

/* MQTT 相关参数 */
static struct mosquitto *mosq = NULL;
static const char *device_control_topic =
    "DeviceControl/SeriesRobot"; // 末端位置控制话题
static const char *axis_control_topic = "AxisControl/SeriesRobot"; // 轴控制话题
static const char *device_status_topic =
    "DeviceStatus/SeriesRobot"; // 状态发送话题（末端控制）
static const char *axis_status_topic =
    "AxisStatus/SeriesRobot"; // 状态发送话题 (轴控制)

/* 全局控制话题 */
static const char *global_halt_topic = "GlobalControl/Halt";
static const char *global_disable_topic = "GlobalControl/DisableVoltage";

/* 发送线程句柄 */
static pthread_t send_thread;

/* 全局变量用于存储client_id和msg_id */
static char current_client_id[256] = {0};
static char current_msg_id[256] = {0};
static int current_control_type = 0; // 0=未知, 1=设备控制, 2=轴控制

/* 新增：全局变量记录最后接收到的控制类型 */
static int last_received_control_type =
    0; // 0=未知, 1=DeviceControl, 2=AxisControl

//  用于存储上一个成功处理的msg_id
static char last_processed_msg_id[256] = {0};

/* 内部函数声明 */
static void on_connect(struct mosquitto *mosq, void *obj, int rc);
static void on_message(struct mosquitto *mosq, void *obj,
                       const struct mosquitto_message *msg);
static void *send_thread_func(void *arg);
static void send_ack_message(const char *msg_id, int status, int control_type);
static void handle_device_control_message(cJSON *root);
static void handle_axis_control_message(cJSON *root);
static void handle_global_halt(void);
static void handle_global_disable_voltage(void);
/**
 * @brief 发送ACK确认消息
 */
static void send_ack_message(const char *msg_id, int status, int control_type) {
  if (!mosq || !msg_id || strlen(current_client_id) == 0) {
    return;
  }

  cJSON *root = cJSON_CreateObject();
  if (!root) {
    fprintf(stderr, "创建ACK JSON对象失败\n");
    return;
  }

  cJSON_AddStringToObject(root, "msg_id", msg_id);
  cJSON_AddNumberToObject(root, "status", status);

  char *json_str = cJSON_PrintUnformatted(root);
  if (json_str) {
    char ack_topic[512];
    if (control_type == 1) {
      snprintf(ack_topic, sizeof(ack_topic), "DeviceControlAck/%s/SeriesRobot",
               current_client_id);
    } else if (control_type == 2) {
      snprintf(ack_topic, sizeof(ack_topic), "AxisControlAck/%s/SeriesRobot",
               current_client_id);
    } else {
      free(json_str);
      cJSON_Delete(root);
      return;
    }

    int ret = mosquitto_publish(mosq, NULL, ack_topic, (int)strlen(json_str),
                                json_str, 0, false);
    if (ret != MOSQ_ERR_SUCCESS) {
      fprintf(stderr, "发布ACK消息失败: %s\n", mosquitto_strerror(ret));
    } else {
      printf("发送ACK到话题: %s, 内容: %s\n", ack_topic, json_str);
    }
    free(json_str);
  }

  cJSON_Delete(root);
}

/**
 * @brief 处理设备控制消息（末端位置控制）- 支持绝对和增量控制
 */
static void handle_device_control_message(cJSON *root) {
  // 提取client_id和msg_id
  cJSON *client_id = cJSON_GetObjectItem(root, "client_id");
  cJSON *msg_id = cJSON_GetObjectItem(root, "msg_id");

  // --- 运行中状态检查 ---
  if (cJSON_IsString(client_id)) {
    strncpy(current_client_id, client_id->valuestring,
            sizeof(current_client_id) - 1);
  }

  if (g_robotStateData.taskState == 0) { // 运行中
    if (cJSON_IsString(msg_id) && strlen(msg_id->valuestring) > 0) {
      send_ack_message(msg_id->valuestring, 105, 1);
      printf("运行中，已对 client_id=%s 的 msg_id=%s 返回 "
             "status=105（DeviceControl）\n",
             current_client_id, msg_id->valuestring);
    } else {
      printf("运行中，收到无 msg_id 的 DeviceControl 消息，未发送 105 ACK\n");
    }
    return;
  }

  // --- 重复msg_id检查 ---
  if (cJSON_IsString(msg_id) && strlen(msg_id->valuestring) > 0 &&
      strcmp(msg_id->valuestring, last_processed_msg_id) == 0) {
    printf("检测到重复的 msg_id: %s，指令被忽略（不发送 ACK）。\n",
           msg_id->valuestring);
    if (cJSON_IsString(client_id)) {
      strncpy(current_client_id, client_id->valuestring,
              sizeof(current_client_id) - 1);
    }
    duplicate_msg_active = 1;
    duplicate_control_type = 1;
    last_received_control_type = 1;
    return;
  }

  if (cJSON_IsString(client_id)) {
    strncpy(current_client_id, client_id->valuestring,
            sizeof(current_client_id) - 1);
  }
  if (cJSON_IsString(msg_id)) {
    strncpy(current_msg_id, msg_id->valuestring, sizeof(current_msg_id) - 1);
  }

  current_control_type = 1;
  last_received_control_type = 1;

  // 提取move_type
  cJSON *move_type = cJSON_GetObjectItem(root, "move_type");
  if (!cJSON_IsString(move_type)) {
    printf("无效的move_type\n");
    send_ack_message(current_msg_id, 0, current_control_type);
    return;
  }

  if (strcmp(move_type->valuestring, "absolute") == 0) {
    // ====== 绝对位置控制（原有逻辑） ======
    cJSON *parameter = cJSON_GetObjectItem(root, "parameter");
    if (cJSON_IsArray(parameter) && cJSON_GetArraySize(parameter) >= 6) {
      for (int i = 0; i < 6; i++) {
        g_robotControlData.AbsolutePose[i] =
            cJSON_GetArrayItem(parameter, i)->valuedouble;
      }
    }
    // 1. 首先，为 option 中的所有参数设置默认值
    g_robotControlData.step = 10; // 默认 step 为 10
    for (int i = 0; i < 6; i++) {
      g_robotControlData.Delta[i] = 0.0; // 默认 obj_pos (Delta) 全为 0
    }
    // 2. 尝试获取 option 对象并覆盖默认值
    cJSON *option = cJSON_GetObjectItem(root, "option");
    // 检查 option 是否存在且是一个有效的 JSON 对象
    if (option && cJSON_IsObject(option)) {
      cJSON *obj_pos = cJSON_GetObjectItem(option, "obj_pos");
      if (cJSON_IsArray(obj_pos) && cJSON_GetArraySize(obj_pos) >= 6) {
        // 如果找到了 obj_pos，就用它的值覆盖默认值
        for (int i = 0; i < 6; i++) {
          g_robotControlData.Delta[i] =
              cJSON_GetArrayItem(obj_pos, i)->valuedouble;
        }
      }

      cJSON *step = cJSON_GetObjectItem(option, "step");
      if (cJSON_IsNumber(step)) {
        // 如果找到了 step，就用它的值覆盖默认值
        g_robotControlData.step = step->valueint;
      }
    }

    g_robotControlData.taskType = 2; // 末端位置控制（绝对）
    g_robotControlData.taskNum++;

    // 更新全局 obj_pos，供正运动学使用
    memcpy(g_current_obj_pos, g_robotControlData.Delta,
           sizeof(g_current_obj_pos));

    printf("接收到设备控制命令 - 微管末端绝对位置控制\n");
    printf(
        "parameter(微管末端位姿): [%.3f, %.3f, %.3f, %.3f, %.3f, %.3f]\n",
        g_robotControlData.AbsolutePose[0], g_robotControlData.AbsolutePose[1],
        g_robotControlData.AbsolutePose[2], g_robotControlData.AbsolutePose[3],
        g_robotControlData.AbsolutePose[4], g_robotControlData.AbsolutePose[5]);
    printf("obj_pos(TCP到微管变换): [%.3f, %.3f, %.3f, %.3f, %.3f, %.3f]\n",
           g_robotControlData.Delta[0], g_robotControlData.Delta[1],
           g_robotControlData.Delta[2], g_robotControlData.Delta[3],
           g_robotControlData.Delta[4], g_robotControlData.Delta[5]);

  } else if (strcmp(move_type->valuestring, "relative") == 0) {
    // ====== 增量位置控制（新增逻辑） ======
    cJSON *parameter = cJSON_GetObjectItem(root, "parameter");
    if (cJSON_IsArray(parameter) && cJSON_GetArraySize(parameter) >= 6) {
      // parameter为增量值，存储到AbsolutePose中
      for (int i = 0; i < 6; i++) {
        g_robotControlData.AbsolutePose[i] =
            cJSON_GetArrayItem(parameter, i)->valuedouble;
      }
    }

    // 1. 首先，为 option 中的所有参数设置默认值
    g_robotControlData.step = 10; // 默认 step 为 10
    for (int i = 0; i < 6; i++) {
      g_robotControlData.Delta[i] = 0.0; // 默认 obj_pos (Delta) 全为 0
    }

    cJSON *option = cJSON_GetObjectItem(root, "option");
    if (option && cJSON_IsObject(option)) {
      cJSON *obj_pos = cJSON_GetObjectItem(option, "obj_pos");
      if (cJSON_IsArray(obj_pos) && cJSON_GetArraySize(obj_pos) >= 6) {
        for (int i = 0; i < 6; i++) {
          g_robotControlData.Delta[i] =
              cJSON_GetArrayItem(obj_pos, i)->valuedouble;
        }
      }

      cJSON *step = cJSON_GetObjectItem(option, "step");
      if (cJSON_IsNumber(step)) {
        g_robotControlData.step = step->valueint;
      }
    }

    g_robotControlData.taskType = 6; // 末端位置控制（增量）- 新的任务类型
    g_robotControlData.taskNum++;

    // 更新全局 obj_pos，供正运动学使用
    memcpy(g_current_obj_pos, g_robotControlData.Delta,
           sizeof(g_current_obj_pos));

    printf("接收到设备控制命令 - 微管末端增量位置控制\n");
    printf(
        "parameter(增量值): [%.3f, %.3f, %.3f, %.3f, %.3f, %.3f]\n",
        g_robotControlData.AbsolutePose[0], g_robotControlData.AbsolutePose[1],
        g_robotControlData.AbsolutePose[2], g_robotControlData.AbsolutePose[3],
        g_robotControlData.AbsolutePose[4], g_robotControlData.AbsolutePose[5]);
    printf("obj_pos(TCP到微管变换): [%.3f, %.3f, %.3f, %.3f, %.3f, %.3f]\n",
           g_robotControlData.Delta[0], g_robotControlData.Delta[1],
           g_robotControlData.Delta[2], g_robotControlData.Delta[3],
           g_robotControlData.Delta[4], g_robotControlData.Delta[5]);

  } else if (strcmp(move_type->valuestring, "halt") == 0) {
    g_robotControlData.taskType = 5; // 暂停模式
    g_robotControlData.taskNum++;
    printf("接收到暂停命令\n");

  } else if (strcmp(move_type->valuestring, "disable_voltage") == 0) {
    g_robotControlData.taskType = 4; // 急停模式
    g_robotControlData.taskNum++;
    printf("接收到停机命令\n");

  } else {
    printf("未知的move_type: %s\n", move_type->valuestring);
    send_ack_message(current_msg_id, 0, current_control_type);
    return;
  }

  // 更新last_processed_msg_id
  strncpy(last_processed_msg_id, current_msg_id,
          sizeof(last_processed_msg_id) - 1);
  // 接受到新的有效指令，清除重复标志
  duplicate_msg_active = 0;
  duplicate_control_type = 0;
}

/**
 * @brief 处理轴控制消息
 */
static void handle_axis_control_message(cJSON *root) {
  // 提取client_id和msg_id
  cJSON *client_id = cJSON_GetObjectItem(root, "client_id");
  cJSON *msg_id = cJSON_GetObjectItem(root, "msg_id");

  // --- 新增：如果正在运行中（taskState == 0），收到新的控制消息则立即回复
  // status=105 ---
  if (cJSON_IsString(client_id)) {
    strncpy(current_client_id, client_id->valuestring,
            sizeof(current_client_id) - 1);
  }

  if (g_robotStateData.taskState == 0) { // 运行中
    if (cJSON_IsString(msg_id) && strlen(msg_id->valuestring) > 0) {
      send_ack_message(msg_id->valuestring, 105, 2);
      printf("运行中，已对 client_id=%s 的 msg_id=%s 返回 "
             "status=105（AxisControl）\n",
             current_client_id, msg_id->valuestring);
    } else {
      printf("运行中，收到无 msg_id 的 AxisControl 消息，未发送 105 ACK\n");
    }
    return;
  }
  // --- 运行中处理结束 ---

  // --- 【新增功能】: 重复msg_id检查 ---
  if (cJSON_IsString(msg_id) && strlen(msg_id->valuestring) > 0 &&
      strcmp(msg_id->valuestring, last_processed_msg_id) == 0) {
    printf("检测到重复的 msg_id: %s，指令被忽略（不发送 ACK）。\n",
           msg_id->valuestring);
    if (cJSON_IsString(client_id)) {
      strncpy(current_client_id, client_id->valuestring,
              sizeof(current_client_id) - 1);
    }
    // 设置重复标志：不发送 ACK，而是持续在状态消息中上报 status=103
    duplicate_msg_active = 1;
    duplicate_control_type = 2;     // axis control
    last_received_control_type = 2; // 确保发送线程发到 AxisStatus 话题
    return;                         // 终止本次处理
  }

  // --- 检查结束 ---

  if (cJSON_IsString(client_id)) {
    strncpy(current_client_id, client_id->valuestring,
            sizeof(current_client_id) - 1);
  }
  if (cJSON_IsString(msg_id)) {
    strncpy(current_msg_id, msg_id->valuestring, sizeof(current_msg_id) - 1);
  }

  current_control_type = 2;       // 轴控制
  last_received_control_type = 2; // 记录最后接收到的控制类型

  // 提取move_type
  cJSON *move_type = cJSON_GetObjectItem(root, "move_type");
  if (!cJSON_IsString(move_type)) {
    printf("无效的move_type\n");
    send_ack_message(current_msg_id, 0, current_control_type);
    return;
  }

  if (strcmp(move_type->valuestring, "absolute") == 0) {
    // parameter是6轴关节值 [rz,y,x,z,rx,ry] (机器人从下到上六轴)
    cJSON *parameter = cJSON_GetObjectItem(root, "parameter");
    if (cJSON_IsArray(parameter) && cJSON_GetArraySize(parameter) >= 6) {
      for (int i = 0; i < 6; i++) {
        g_robotControlData.Delta[i] =
            cJSON_GetArrayItem(parameter, i)->valuedouble;
      }
    }

    // 清空AbsolutePose，因为这是关节控制不需要末端位姿
    for (int i = 0; i < 6; i++) {
      g_robotControlData.AbsolutePose[i] = 0.0;
    }

    // 1. 首先，设置 step 的默认值
    g_robotControlData.step = 10;
    // 2. 尝试获取 option 对象并覆盖默认值
    cJSON *option = cJSON_GetObjectItem(root, "option");
    if (option && cJSON_IsObject(option)) {
      cJSON *step = cJSON_GetObjectItem(option, "step");
      if (cJSON_IsNumber(step)) {
        g_robotControlData.step = step->valueint;
      }
    }

    // 设置任务类型为关节控制
    g_robotControlData.taskType = 1; // 关节控制
    g_robotControlData.taskNum++;    // 递增任务编号

    // 轴控制时，obj_pos 保持为0（或保持上次的值）
    // 这里不修改 g_current_obj_pos，保持之前的值

    printf("接收到轴控制命令 - 关节位置控制\n");
    printf(
        "目标关节位置[rz,y,x,z,rx,ry]: [%.3f, %.3f, %.3f, %.3f, %.3f, %.3f]\n",
        g_robotControlData.Delta[0], g_robotControlData.Delta[1],
        g_robotControlData.Delta[2], g_robotControlData.Delta[3],
        g_robotControlData.Delta[4], g_robotControlData.Delta[5]);

  } else if (strcmp(move_type->valuestring, "relative") == 0) {
    // parameter 是6轴的增量值 [rz,y,x,z,rx,ry] (单位: 度 / 微米)
    cJSON *parameter = cJSON_GetObjectItem(root, "parameter");
    if (cJSON_IsArray(parameter) && cJSON_GetArraySize(parameter) >= 6) {
      for (int i = 0; i < 6; i++) {
        g_robotControlData.Delta[i] =
            cJSON_GetArrayItem(parameter, i)->valuedouble;
      }
    }

    // 清空AbsolutePose，因为这是关节控制不需要末端位姿
    for (int i = 0; i < 6; i++) {
      g_robotControlData.AbsolutePose[i] = 0.0;
    }

    // 1. 首先，设置 step 的默认值
    g_robotControlData.step = 10;
    // 2. 尝试获取 option 对象并覆盖默认值
    cJSON *option = cJSON_GetObjectItem(root, "option");

    if (option && cJSON_IsObject(option)) {
      cJSON *step = cJSON_GetObjectItem(option, "step");
      if (cJSON_IsNumber(step)) {
        g_robotControlData.step = step->valueint;
      }
    }
    // 设置任务类型为相对关节控制
    g_robotControlData.taskType = 7; // 新的任务类型，用于相对轴控制
    g_robotControlData.taskNum++;    // 递增任务编号

    printf("接收到轴控制命令 - 关节相对位置控制\n");
    printf("目标关节增量[rz,y,x,z,rx,ry]: [%.3f, %.3f, %.3f, %.3f, %.3f, "
           "%.3f]\n",
           g_robotControlData.Delta[0], g_robotControlData.Delta[1],
           g_robotControlData.Delta[2], g_robotControlData.Delta[3],
           g_robotControlData.Delta[4], g_robotControlData.Delta[5]);

  } else if (strcmp(move_type->valuestring, "halt") == 0) {
    g_robotControlData.taskType = 5; // 暂停模式
    g_robotControlData.taskNum++;
    printf("接收到暂停命令\n");

  } else if (strcmp(move_type->valuestring, "disable_voltage") == 0) {
    g_robotControlData.taskType = 4; // 急停模式
    g_robotControlData.taskNum++;
    printf("接收到停机命令\n");

  } else {
    printf("未知的move_type: %s\n", move_type->valuestring);
    send_ack_message(current_msg_id, 0, current_control_type);
    return;
  }
  // --- 【新增功能】: 指令有效且被接受，更新last_processed_msg_id ---
  strncpy(last_processed_msg_id, current_msg_id,
          sizeof(last_processed_msg_id) - 1);
  // 接受到新的有效指令，清除重复标志（停止持续上报103）
  duplicate_msg_active = 0;
  duplicate_control_type = 0;
}

/**
 * @brief 处理全局暂停命令 (GlobalControl/Halt)
 *
 * 当收到此话题时，无论消息体是否为空，都执行暂停。
 */
static void handle_global_halt(void) {
  printf("接收到全局暂停命令 (GlobalControl/Halt)\n");
  // 设置任务类型为暂停模式
  // term.c 中的 process_main 会检测到这个变化
  g_robotControlData.taskType = 5;
  g_robotControlData.taskNum++; // 增加任务编号以触发状态机
}

/**
 * @brief 处理全局停机命令 (GlobalControl/DisableVoltage)
 *
 * 当收到此话题时，无论消息体是否为空，都执行急停。
 */
static void handle_global_disable_voltage(void) {
  printf("接收到全局停机命令 (GlobalControl/DisableVoltage)\n");
  // 设置任务类型为急停模式
  // term.c 中的 process_main 会检测到这个变化
  g_robotControlData.taskType = 4;
  g_robotControlData.taskNum++; // 增加任务编号以触发状态机
}

//打印cjson
void print_robot_control_data(const RobotControlData *data) {
  /* 创建 JSON 根对象 */
  cJSON *root = cJSON_CreateObject();
  if (!root) {
    fprintf(stderr, "创建 JSON 对象失败\n");
    return;
  }

  /* 添加绝对位置 AbsolutePose 数组 */
  cJSON *absPose = cJSON_CreateDoubleArray(data->AbsolutePose, 6);
  if (!absPose) {
    fprintf(stderr, "创建 AbsolutePose 数组失败\n");
    cJSON_Delete(root);
    return;
  }
  cJSON_AddItemToObject(root, "AbsolutePose", absPose);

  /* 添加增量位置信息 IncPosition 对象 */
  cJSON *incPosition = cJSON_CreateObject();
  if (!incPosition) {
    fprintf(stderr, "创建 IncPosition 对象失败\n");
    cJSON_Delete(root);
    return;
  }
  for (int i = 0; i < 6; i++) {
    char key[16];
    snprintf(key, sizeof(key), "Delta%d", i + 1);
    cJSON_AddNumberToObject(incPosition, key, data->Delta[i]);
  }
  cJSON_AddNumberToObject(incPosition, "Step", data->step);
  cJSON_AddItemToObject(root, "IncPosition", incPosition);

  /* 添加任务信息 TaskInfo 对象 */
  cJSON *taskInfo = cJSON_CreateObject();
  if (!taskInfo) {
    fprintf(stderr, "创建 TaskInfo 对象失败\n");
    cJSON_Delete(root);
    return;
  }
  cJSON_AddNumberToObject(taskInfo, "TaskNum", data->taskNum);
  cJSON_AddNumberToObject(taskInfo, "TaskType", data->taskType);
  cJSON_AddItemToObject(root, "TaskInfo", taskInfo);

  /* 打印生成的 JSON 字符串 */
  char *json_str = cJSON_PrintUnformatted(root);
  if (json_str) {
    printf("MQTT JSON 消息格式:\n%s\n", json_str);
    free(json_str);
  }

  cJSON_Delete(root);
}

/**
 * @brief 初始化 MQTT 客户端、建立连接并启动后台线程
 */
int mqtt_comm_init(const char *broker_address, int broker_port) {
  int ret;
  mosquitto_lib_init();
  mosq = mosquitto_new(NULL, true, NULL);
  if (!mosq) {
    fprintf(stderr, "创建 mosquitto 客户端失败\n");
    return -1;
  }
  /* 设置回调函数 */
  mosquitto_connect_callback_set(mosq, on_connect);
  mosquitto_message_callback_set(mosq, on_message);

  ret = mosquitto_connect(mosq, broker_address, broker_port, 60);
  if (ret) {
    fprintf(stderr, "连接失败, 错误码：%d\n", ret);
    return ret;
  }
  /* 启动网络处理线程（内部会创建一个线程） */
  ret = mosquitto_loop_start(mosq);
  if (ret) {
    fprintf(stderr, "启动 mosquitto 网络循环失败：%d\n", ret);
    return ret;
  }
  /* 创建发送线程，每秒发布一次状态数据 */
  ret = pthread_create(&send_thread, NULL, send_thread_func, NULL);
  if (ret) {
    fprintf(stderr, "创建发送线程失败\n");
    return ret;
  }
  return 0;
}

/**
 * @brief 清理 MQTT 客户端资源
 */
void mqtt_comm_cleanup(void) {
  if (mosq) {
    mosquitto_disconnect(mosq);
    mosquitto_loop_stop(mosq, true);
    mosquitto_destroy(mosq);
  }
  mosquitto_lib_cleanup();
}

/**
 * @brief
 * 阻塞等待发送线程结束（示例中发送线程无限循环，可根据需要修改退出机制）
 */
void mqtt_comm_start(void) { pthread_detach(send_thread); }

/**
 * @brief MQTT 连接成功后的回调函数，订阅多个话题
 */
static void on_connect(struct mosquitto *mosq, void *obj, int rc) {
  if (rc == 0) {
    printf("成功连接到 MQTT 服务器。\n");
    // 订阅设备控制和轴控制话题
    mosquitto_subscribe(mosq, NULL, device_control_topic, 0);
    mosquitto_subscribe(mosq, NULL, axis_control_topic, 0);
    printf("已订阅话题: %s 和 %s\n", device_control_topic, axis_control_topic);
    /* 订阅全局控制话题 */
    mosquitto_subscribe(mosq, NULL, global_halt_topic, 0);
    mosquitto_subscribe(mosq, NULL, global_disable_topic, 0);
    printf("已订阅全局话题: %s 和 %s\n", global_halt_topic,
           global_disable_topic);
  } else {
    printf("连接失败，错误码：%d\n", rc);
  }
}

/**
 * @brief 收到消息后的回调函数
 *
 * 根据话题类型分别处理设备控制和轴控制消息
 */
static void on_message(struct mosquitto *mosq, void *obj,
                       const struct mosquitto_message *msg) {
  (void)obj;
  if (!msg->payloadlen) {
    return;
  }

  // 忽略 retained 消息（通常是 Broker 上残留的旧命令）
  if (msg->retain) {
    printf("忽略保留消息 (retain)\n");
    return;
  }

  printf("收到话题 %s 的消息: %s\n", msg->topic, (char *)msg->payload);

  // +++ 新增：首先处理全局非JSON话题 +++
  // 这些话题不需要解析JSON，可以直接处理
  if (strcmp(msg->topic, global_halt_topic) == 0) {
    handle_global_halt();
    return; // 处理完毕

  } else if (strcmp(msg->topic, global_disable_topic) == 0) {
    handle_global_disable_voltage();
    return; // 处理完毕
  }
  // --- 全局话题处理结束 ---

  // 对于非全局话题，payload 不能为空
  if (!msg->payloadlen) {
    printf("收到空 payload，但非全局话题，忽略。\n");
    return;
  }

  cJSON *root = cJSON_Parse(msg->payload);
  if (root == NULL) {
    fprintf(stderr, "JSON解析失败\n");
    return;
  }

  // 根据话题类型处理消息
  if (strcmp(msg->topic, device_control_topic) == 0) {
    handle_device_control_message(root);
  } else if (strcmp(msg->topic, axis_control_topic) == 0) {
    handle_axis_control_message(root);
  } else {
    printf("未知话题: %s\n", msg->topic);
    // cJSON_Delete(root);
    // return;
  }

  cJSON_Delete(root);
}

/**
 * @brief 发送线程函数
 *
 * 每隔1秒同时发送两种状态消息：
 * 1. 始终发布到 AxisStatus/SeriesRobot (关节状态)
 * 2. 始终发布到 DeviceStatus/SeriesRobot (末端状态)
 */
static void *send_thread_func(void *arg) {
  (void)arg;
  static int last_task_state = -99; // 用于检测状态变化

  while (1) {
    // 1. 计算外部状态 (公共逻辑)
    int external_status;
    if (duplicate_msg_active) {
      // 检测到重复 msg_id，按照要求上报 status=103（持续上报）
      external_status = 103;
    } else {
      // 原有逻辑
      switch (g_robotStateData.taskState) {
      case 1:
        external_status = 1;
        break; // 任务完成
      case 0:
        external_status = 0;
        break; // 任务执行中
      case 104:
        external_status = 104; // 新增：指令参数超范围
        break;
      case 5:
        external_status = 301;
        break;  // 暂停
      case 201: // 急停
      case 202:
      case 203:
      case 204:
        external_status = 300;
        break;
      default:
        external_status = 0;
        break;
      }
    }

    // 2. 发送轴状态消息到 AxisStatus/SeriesRobot
    {
      cJSON *axis_root = cJSON_CreateObject();
      if (axis_root) {
        cJSON_AddNumberToObject(axis_root, "status", external_status);

        // 准备 parameter: 上传关节位置 [rz,y,x,z,rx,ry] (米/度 转 微米/度)
        double jointPosition[6];
        jointPosition[0] = g_robotStateData.PositionAxis[0]; // rz (度)
        jointPosition[1] =
            g_robotStateData.PositionAxis[1] * 1000000.0; // y (m -> μm)
        jointPosition[2] =
            g_robotStateData.PositionAxis[2] * 1000000.0; // x (m -> μm)
        jointPosition[3] =
            g_robotStateData.PositionAxis[3] * 1000000.0;    // z (m -> μm)
        jointPosition[4] = g_robotStateData.PositionAxis[4]; // rx (度)
        jointPosition[5] = g_robotStateData.PositionAxis[5]; // ry (度)

        cJSON *axis_parameter = cJSON_CreateDoubleArray(jointPosition, 6);
        cJSON_AddItemToObject(axis_root, "parameter", axis_parameter);

        // 注意：轴状态不添加 "option" 和 "obj_pos"

        // 发布轴状态消息
        char *axis_json_str = cJSON_PrintUnformatted(axis_root);
        if (axis_json_str) {
          int ret = mosquitto_publish(mosq, NULL, axis_status_topic,
                                      (int)strlen(axis_json_str), axis_json_str,
                                      0, false);
          if (ret != MOSQ_ERR_SUCCESS) {
            fprintf(stderr, "发布轴状态消息到 %s 失败: %s\n", axis_status_topic,
                    mosquitto_strerror(ret));
          }
          free(axis_json_str);
        }
        cJSON_Delete(axis_root);
      }
    }

    // 3. 发送末端状态消息到 DeviceStatus/SeriesRobot
    {
      cJSON *device_root = cJSON_CreateObject();
      if (device_root) {
        cJSON_AddNumberToObject(device_root, "status", external_status);

        // 准备 parameter: 上传末端位置 [x, y, z, Rx, Ry, Rz]
        double endPosition[6];
        endPosition[0] = g_robotStateData.PositionXYZ[0]; // x
        endPosition[1] = g_robotStateData.PositionXYZ[1]; // y
        endPosition[2] = g_robotStateData.PositionXYZ[2]; // z
        endPosition[3] = g_robotStateData.PositionXYZ[3]; // Rx
        endPosition[4] = g_robotStateData.PositionXYZ[4]; // Ry
        endPosition[5] = g_robotStateData.PositionXYZ[5]; // Rz

        cJSON *device_parameter = cJSON_CreateDoubleArray(endPosition, 6);
        cJSON_AddItemToObject(device_root, "parameter", device_parameter);

        // 添加 "option" 对象和 "obj_pos"
        cJSON *option = cJSON_CreateObject();
        cJSON *obj_pos = cJSON_CreateDoubleArray(g_current_obj_pos, 6);
        cJSON_AddItemToObject(option, "obj_pos", obj_pos);
        cJSON_AddItemToObject(device_root, "option", option);

        // 发布末端状态消息
        char *device_json_str = cJSON_PrintUnformatted(device_root);
        if (device_json_str) {
          int ret = mosquitto_publish(mosq, NULL, device_status_topic,
                                      (int)strlen(device_json_str),
                                      device_json_str, 0, false);
          if (ret != MOSQ_ERR_SUCCESS) {
            fprintf(stderr, "发布末端状态消息到 %s 失败: %s\n",
                    device_status_topic, mosquitto_strerror(ret));
          }
          free(device_json_str);
        }
        cJSON_Delete(device_root);
      }
    }

    // 4. 检查是否需要发送ACK消息 (保持原有逻辑)
    if (g_robotStateData.taskState == 1 && last_task_state != 1 &&
        strlen(current_msg_id) > 0 && current_control_type > 0) {
      send_ack_message(current_msg_id, 1, current_control_type);
      memset(current_msg_id, 0, sizeof(current_msg_id));
      current_control_type = 0;
    }

    last_task_state = g_robotStateData.taskState;
    sleep(1);
  }
  return NULL;
}